/*********************************************************************************
 *FileName:  bdc.c
 *Author:    ChenJiehong
 *Version:   V1.0
 *Date:      2023-09-23
 ****************************************Includes***********************************/
#include "bdc.h"
#include "fault_diagnosis.h"
#include "swtimer.h"

Actuator_Parameter_t Actuator;
uint8_t g_u8OverCurrent_Cnt = 0;

FILTER_PID_Object_t l_sPositionPidObject = {
    .u16KP = POSITION_PID_P,
    .u16KI = POSITION_PID_I,
    .u16KD = POSITION_PID_D,

    .i32Error_Sum = POSITION_PID_EERORSUM,
    .Integral_LimitMax = POSITION_PID_EERORSUM_MAX,
    .i32Integral = POSITION_PID_INTEGRAL,
    .i16LastError = POSITION_PID_LAST_ERROR,
    .u16OutputLimitMax = POSITION_PID_PWM_DUTY_MAX,
    .u16OutputLimitMin = POSITION_PID_PWM_DUTY_MIN,
};
/***************************************Variables***********************************/

/***************************************Functions***********************************/


void Hard_Current_Limit_BEMFC_Init(void)
{	
	BEMFC_InitTypeDef BEMFC_InitStruct;
	Reference_InitTypeDef Reference_InitStruct;
	RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DAC, ENABLE);
	SYSCTRL->ANACR_b.REFEN = 1;
	SYSCTRL->ANACR_b.ISNGC = 3;//1400mA
	
	BEMFC_InitStruct.BEMFC_FilterTime = BEMFC_FilterTime_0us6_0us8;
	BEMFC_InitStruct.BEMFC_OutDiretion = BMFC_Out_NO_Opposite;
	BEMFC_InitStruct.BEMFC_BlankingTime = BEMFC_BlankingTime_Disable;
	BEMFC_InitStruct.BEMFC_EventSource = BEMFC_Event_Source_0;
	BEMFC_InitStruct.BEMFC_EventTest = EventTest_ALL;
	BEMFC_InitStruct.BEMFC_Input = BEMFC_Input_Current;
	BEMFC0_Init(BEMFC,&BEMFC_InitStruct);
	BEMFC1_Init(BEMFC,&BEMFC_InitStruct);
	
	BEMFC0_Cmd(BEMFC,ENABLE);
	BEMFC1_Cmd(BEMFC,ENABLE);
	
	Reference_InitStruct.Reference_Mode = BEMFC_ReferenceMode_Voltage;
	Reference_InitStruct.Current_Reference = Current_ReferenceMode0;
	Reference_InitStruct.Channel0_Current_Source = BEMFC_CurrentSource0;//LS[0]
	Reference_InitStruct.Channel1_Current_Source = BEMFC_CurrentSource2;//LS[2]
	BEMFC_Congfig(BEMFC,&Reference_InitStruct);
	
	CLDAC_BufferCmd(BEMFC,ENABLE);
	CLDAC_TRG0_EeventCmd(BEMFC,DISABLE);
	CLDAC_EOP0_EeventCmd(BEMFC,ENABLE);
	
	CLDAC_Cmd(BEMFC,ENABLE);//设定限流值前必须先使能CLDAC
	CLDAC0_Current_Limit(BEMFC,HARD_CURRENT_LIMIT_SET);
	CLDAC1_Current_Limit(BEMFC,HARD_CURRENT_LIMIT_SET);
}

/**
 * @brief 电机驱动初始化函数
 * @param None
 * @retval None
 */
void MotorDriver_Init(void)
{
    ChargePum_InitTypeDef ChargePum_InitStruct;
    EPWM_InitTypeDef EPWM_InitStruct;

    HalfBridge_InitTypeDef EPWM0_HalfBridge_InitStruct;
    HalfBridge_InitTypeDef EPWM2_HalfBridge_InitStruct;

    RCC_AHBPeriphClockCmd(RCC_AHBPeriph_PWM, ENABLE);

    ChargePum_InitStruct.ChargePum_Interrupt = DISABLE;
    ChargePum_InitStruct.ChargePum_Filter = DISABLE;
    ChargePum_InitStruct.ChargePum_FilterTime = ChargePump_Filter_1us;
    ChargePum_InitStruct.ChargePum_OVShuntDown = ENABLE;
    ChargePump_Init(&ChargePum_InitStruct);
    ChargePump_Cmd(ENABLE);

	/* EPWM_SlewRate_Slow 模式电流采不准 会有尖峰 暂时还未找到原因 */
    EPWM_InitStruct.EPWM_OFF_SlewRate = EPWM_SlewRate_Normal; //EPWM_SlewRate_Slow
    EPWM_InitStruct.EPWM_ON_SlewRate = EPWM_SlewRate_Normal;
    EPWM_InitStruct.EPWM_OC_FilterTime = EPWM_OC_FilterTime_2us4;
    EPWM_InitStruct.EPWM_ClockSource = EPWM_CLK_SOURCE;
    EPWM_InitStruct.EPWM_CC_ProtectionTime = EPWM_CCPT_625ns;
    EPWM_InitStruct.EPWM_OC_ShutDownMode = EPWM_OCSDM_ALL;
    EPWM_InitStruct.EPWM_BridgeArmMode = EPWM_BridgeArm_Couple;
    EPWM_InitStruct.EPWM_CounterMode = EPWM_COUNTER_MODE;
    EPWM_Init(EPWM, &EPWM_InitStruct);

    EPWM0_HalfBridge_InitStruct.HalfBridge_CurrentLimit = ENABLE;
    EPWM0_HalfBridge_InitStruct.HalfBridge_CurrentLimitSource = EPWM_CLS_BEMFC_CH1;
    EPWM0_HalfBridge_InitStruct.HalfBridge_LowSideOutMode = EPWM_BridgeArm_OFF;
    EPWM0_HalfBridge_InitStruct.HalfBridge_HighSideOutMode = EPWM_BridgeArm_OFF;
    EPWM0_HalfBridge_InitStruct.HalfBridge_Period = EPWM_PERIOD;
    EPWM0_HalfBridge_InitStruct.HalfBridge_Compare = 0;
    EPWM_HalfBridge0_Init(EPWM, &EPWM0_HalfBridge_InitStruct);

    EPWM2_HalfBridge_InitStruct.HalfBridge_CurrentLimit = ENABLE;
    EPWM2_HalfBridge_InitStruct.HalfBridge_CurrentLimitSource = EPWM_CLS_BEMFC_CH0;
    EPWM2_HalfBridge_InitStruct.HalfBridge_LowSideOutMode = EPWM_BridgeArm_OFF;
    EPWM2_HalfBridge_InitStruct.HalfBridge_HighSideOutMode = EPWM_BridgeArm_OFF;
    EPWM2_HalfBridge_InitStruct.HalfBridge_Period = EPWM_PERIOD;
    EPWM2_HalfBridge_InitStruct.HalfBridge_Compare = 0;
    EPWM_HalfBridge2_Init(EPWM, &EPWM2_HalfBridge_InitStruct);

    EPWM_Trig0ModeConfig(EPWM, EPWM_TrigState_Enable, EPWM_TrigState_Disable, 1);
    EPWM_Trig0TimeConfig(EPWM, 0);

    Hard_Current_Limit_BEMFC_Init();

    EPWM_MinimumOnTime0Cmd(EPWM, DISABLE);
    EPWM_MinimumOnTime2Cmd(EPWM, DISABLE);
    EPWM_MinimumOnTimeConfig(EPWM, 80); 

    EPWM_IT0Config(EPWM, EPWM_IT_OC, ENABLE);
    NVIC_EnableIRQ(EPWM0_IRQn);
    NVIC_SetPriority(EPWM0_IRQn, 4);

    EPWM_IT2Config(EPWM, EPWM_IT_OC, ENABLE);
    NVIC_EnableIRQ(EPWM2_IRQn);
    NVIC_SetPriority(EPWM2_IRQn, 4);

    EPWM_SetCompare0(EPWM, 0);
    EPWM_SetCompare2(EPWM, 0);
    EPWM_Cmd(EPWM, ENABLE);

    swtimer_register(SWTIMER_MOTOR_CTRL_PERIOD, C_PI_TICKS_5MS, TIMER_PERIODIC_MODE);
    swtimer_register(SWTIMER_MOTOR_STOP_DELAY, C_PI_TICKS_250MS, TIMER_ONE_SHOT_MODE);
    swtimer_register(SWTIMER_MOTOR_STALL_TIME, C_PI_TICKS_100MS, TIMER_PERIODIC_MODE);
    swtimer_register(SWTIMER_MOTOR_CALIBRATION_TIME, C_PI_TICKS_100MS, TIMER_PERIODIC_MODE);
}

/**
 * @brief 电机驱动 PWM设置函数
 * @param MotorDir: 电机方向 u16DutyCycle：占空比
 * @retval None
 */
void Motor_PWM_Set(MotorDir_t MotorDir, uint16_t u16DutyCycle)
{
    switch (MotorDir)
    {
    case MOTOR_STOP:
        EPWM_MinimumOnTime0Cmd(EPWM, DISABLE);
        EPWM_MinimumOnTime2Cmd(EPWM, DISABLE);    
        EPWM_SetCompare0(EPWM, 0);
        EPWM_SetCompare2(EPWM, 0);
        break;

    case MOTOR_DIR_CW:
        EPWM_MinimumOnTime0Cmd(EPWM, DISABLE);        
        EPWM_MinimumOnTime2Cmd(EPWM, ENABLE);
        EPWM_HalfBridge0Config(EPWM, EPWM_BridgeArm_ON, EPWM_BridgeArm_OFF);
        EPWM_HalfBridge2Config(EPWM, EPWM_BridgeArm_ReversePWM, EPWM_BridgeArm_PWM);
        EPWM_SetCompare0(EPWM, 0);
        EPWM_SetCompare2(EPWM, u16DutyCycle);
        break;

    case MOTOR_DIR_CCW:
        EPWM_MinimumOnTime0Cmd(EPWM, ENABLE);            
        EPWM_MinimumOnTime2Cmd(EPWM, DISABLE);
        EPWM_HalfBridge0Config(EPWM, EPWM_BridgeArm_ReversePWM, EPWM_BridgeArm_PWM);
        EPWM_HalfBridge2Config(EPWM, EPWM_BridgeArm_ON, EPWM_BridgeArm_OFF);
        EPWM_SetCompare0(EPWM, u16DutyCycle);
        EPWM_SetCompare2(EPWM, 0);
        break;

    default:
        break;
    }
	/* 调整采样点 */
	if(u16DutyCycle<2)
	{
		EPWM_Trig0TimeConfig(EPWM, 1);
	}
	else
	{
		EPWM_Trig0TimeConfig(EPWM, u16DutyCycle/2);
	}
	
}

/**
 * @brief 电机驱动使能
 * @param None
 * @retval None
 */
void MotorDriverEnable(void)
{
    EPWM_HalfBridge0Config(EPWM, EPWM_BridgeArm_ReversePWM, EPWM_BridgeArm_PWM);
    //EPWM_HalfBridge1Config(EPWM, EPWM_BridgeArm_ReversePWM, EPWM_BridgeArm_PWM);
    EPWM_HalfBridge2Config(EPWM, EPWM_BridgeArm_ReversePWM, EPWM_BridgeArm_PWM);
    //EPWM_HalfBridge3Config(EPWM, EPWM_BridgeArm_ReversePWM, EPWM_BridgeArm_PWM);
}

/**
 * @brief 电机驱动禁能
 * @param None
 * @retval None
 */
void MotorDriverTristate(void)
{
    EPWM_HalfBridge0Config(EPWM, EPWM_BridgeArm_OFF, EPWM_BridgeArm_OFF);
    //EPWM_HalfBridge1Config(EPWM, EPWM_BridgeArm_OFF, EPWM_BridgeArm_OFF);
    EPWM_HalfBridge2Config(EPWM, EPWM_BridgeArm_OFF, EPWM_BridgeArm_OFF);
    //EPWM_HalfBridge3Config(EPWM, EPWM_BridgeArm_OFF, EPWM_BridgeArm_OFF);
}

/**
 * @brief 电机驱动启动函数
 * @param None
 * @retval None
 */
void MotorDriverStart(void)
{
    /* 使能电机驱动 */
    MotorDriverEnable();
    /* 电机运行状态更新为运行中 */
    Actuator.Motor.RunStatus = MOTOR_STATUS_RUNNING;

    /* 开启 PID 计算和堵转检测 */
    swtimer_start(SWTIMER_MOTOR_CTRL_PERIOD);
    swtimer_start(SWTIMER_MOTOR_STALL_TIME);
}

/**
 * @brief 电机驱动关闭函数
 * @param None
 * @retval None
 */
void MotorDriverStop(void)
{
    /* 关闭电机驱动，通过下管续流 */
    Motor_PWM_Set(MOTOR_STOP, 0);
    /* 电机运行状态更新为停止中 */
    Actuator.Motor.RunStatus = MOTOR_STATUS_STOPPING;
    Actuator.Motor.Direction = MOTOR_STOP;

    /* 电机停止，清除电机堵转计数 */
    Actuator.Motor.u16MotorRunTimer = 0;

    /* 关闭 PID 计算和堵转检测 */
    swtimer_stop(SWTIMER_MOTOR_CTRL_PERIOD);
    swtimer_stop(SWTIMER_MOTOR_STALL_TIME);
    /* 开启 电机停止 延时定时器 */
    swtimer_start(SWTIMER_MOTOR_STOP_DELAY);
}

/**
 * @brief PID控制函数
 * @param None
 * @retval None
 */
void PID_Control(void)
{
    static uint16_t Stabilization_Cnt;

    /* 计算位置误差 */
    int32_t i32Error = (int32_t)Actuator.u16TargetPosition - Actuator.u16ActualPosition;
    int16_t i16Error;
    if (i32Error >32767)
    {
        i16Error = 32767;
    }
    else if (i32Error < -32768)
    {
        i16Error = -32768;
    }
    else
    {
        i16Error = i32Error;
    }
    
    /* 位置式 PID控制 */
    int16_t i16RetVal = filter_pid_Handler(&l_sPositionPidObject, i16Error);

    /* 根据PID值大小，控制输出方向和大小 */
    if (i16RetVal >= 0)
    {
        if (i16RetVal > Actuator.Motor.u16Duty)
        {
            Actuator.Motor.u16Duty += 50;
        }
        else
        {
            Actuator.Motor.u16Duty = i16RetVal;
        }

        Motor_PWM_Set(MOTOR_DIR_CW, Actuator.Motor.u16Duty);
    }
    else
    {
        if (-i16RetVal > Actuator.Motor.u16Duty)
        {
            Actuator.Motor.u16Duty += 50;
        }
        else
        {
            Actuator.Motor.u16Duty = -i16RetVal;
        }
        Motor_PWM_Set(MOTOR_DIR_CCW, Actuator.Motor.u16Duty);
    }

    /* 位置到达和稳定判断 */
    if (i16Error > POSITION_PID_DEAD || i16Error < (-POSITION_PID_DEAD))
    {
        if (Stabilization_Cnt > 0)
        {
            Stabilization_Cnt--;
        }
    }
    else
    {
        Stabilization_Cnt++;
        if (Stabilization_Cnt > 10)          //与三代英迪芯设置保持一致
        {
            Stabilization_Cnt = 0;
            MotorDriverStop();
						/* 只要能到达目标位置 清除堵转和电机错误标志位*/
						Actuator.Motor.Fault = MOTOR_NO_FAULT;
						Actuator.Motor.StallState = ERR_STALL_NOFOUND;
        }
    }
}

/**
 * @brief 清除过流标志位，否则无法重新启动
 * @param None
 * @retval None
 */
void Clear_Bridge_OverCurrent(void)
{
		/* 清除桥臂 过流状态 */
		EPWM->OC = 0x0F;
}

/**
 * @brief PWM0过流中断服务函数
 * @param None
 * @retval None
 */
void EPWM0_IRQHandler(void)
{
    uint32_t cnt = EPWM->CNT0;
    /*over current interrrupt*/
    if (EPWM->EIPND0_b.OC0 == 1)
    {
        /* 过流处理 */
        OverCurrent_Handle();
				Clear_Bridge_OverCurrent();
        EPWM->EIPND0_b.OC0 = 1;
			
    }
}

/**
 * @brief PWM2过流中断服务函数
 * @param None
 * @retval None
 */
void EPWM2_IRQHandler(void)
{
    uint32_t cnt = EPWM->CNT2;
    /*over current interrrupt*/
    if (EPWM->EIPND2_b.OC2 == 1)
    {
        /* 过流处理 */
        OverCurrent_Handle();
				Clear_Bridge_OverCurrent();
        EPWM->EIPND2_b.OC2 = 1;
    }
}

/***************************************END OF FILE*********************************/
